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A COMPUTER  PROGRAM  FOR  DOUBLE  SWEEP  OPTIMAL  SMOOTHING 
1 . INTRODUCTION 


Purpose. 


The  purpose  of  this  paper  is  to  explain  and  docu- 
ment the  computer  routines  developed  by  the  author  and  used 
by  the  Army  Materiel  Systems  Analysis  Activity  (AMSAA)  for 
the  optimal  smoothing  of  aircraft  tracking  data. 

1.2  Background. 

AMSAA  is  the  independent  evaluator  for  the  Division 
Air  Defense  Gun  (DIVAD)  program.  One  of  the  principal 
features  of  this  program  is  the  use  of  modern  fire  control 
techniques.  These  include  optimal,  Kalman  filtering,  rou- 
tines to  estimate  aircraft  (target)  position,  velocity,  and 
acceleration  from  aircraft  tracking  data.  One  method  used 
to  study  modern  fire  control  consists  of  running  target 
flight  paths  obtained  during  field  tests  through  a digital 
simulation  of  the  fire  control  and  then  comparing  the  esti- 
mates of  the  target  state;  that  is,  target  position,  veloc- 
ity, and  acceleration,  with  the  corresponding  "true"  target 
state  values  (Reference  1).  If  this  comparison  is  to  be 
meaningful,  the  method  used  to  obtain  the  "true"  target 
state  time  history  needs  to  provide  greater  accuracy  than  is 
possible  to  obtain  through  filtering  alone.  The  method 
described  below  guarantees  this  needed  accuracy. 

1.3  Approach. 

Field  test  tracking  data  consisting  of  target 
position  as  a function  of  time  is  processed  in  two  steps. 

It  is  first  filtered  using  a Kalman  filter  which  is  at  least 
as  sophisticated  as  the  filter  to  be  evaluated  (e.g., 
nonlinear,  adaptive,  etc.).  The  target  state  estimates  from 
the  filter  are  then  optimally  corrected  by  sweeping  in 
reverse  chronological  order  through  the  tracking  data.  The 
corrected  estimates  are  necessarily  superior  to  the  filtered 
estimates  since  at  each  instant  of  time,  the  corrected 
estimates  were  computed  using  all  the  tracking  data  while 
the  filtered  estimates  were  derived  using  only  tracking  data 
from  earlier  in  time. 


The  general  theory  of  optimal  smoothing  as  consid- 
ered here  is  fairly  well  established  (References  2 and  3) . 
The  details,  however,  are  often  outside  the  background  of 
analysts  who  could  use  (and  have  used)  this  program  in  other 
Army  studies.  Section  2 provides  a brief  but  complete 
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2 . THEORY 


' 

S I 


2.1  Principal  Definitions. 

• All  vectors  and  matrices  are  expressed  in 
cartesian  coordinates. 

• Superscript  -1,  (-1) , denotes  matrix  inverse. 

• Superscript  bar,  (— ) , denotes  expected  value. 

T 

• Superscript  T,  ( ) denotes  transpose. 

• Subscript  i,  (^)  , denotes  time  step. 

• V,  the  3n+l  dimensional  gradiant  operator,  is 
defined  to  be 


n is  the  maximum  number  of  time  steps  or  the  maxi- 
mum value  of  subscript  i (i.e.,  i goes  from  0 to  n) . 

The  basic  problem  is  to  determine  from  the  tracking 
data  the  position,  velocity,  and  acceleration  or  state 
vector  of  the  target  as  a function  oi  time  in  a manner  which 
is  consistent  with  our  knowledge  of  aircraft  dynamics  and 
measurement  uncertainty.  As  will  be  shown  in  Subsections 
2.2  and  2.3,  the  process  of  determining  estimates  of  the 
target  state  requires  that  estimates  of  several  other  vec- 
tors be  simultaneously  determined.  These  vectors  are  termed 
"derived"  vectors  and  are  defined  in  Table  2-1. 


! 


il 


H 

I 4 

I I 


. 
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TABLE  2-1  DERIVED  VECTORS 


X : The  9 dimensional  (9-D)  state  vector  consisting 
of  target  position,  velocity,  and  acceleration. 
There  are  n + 1 x’s,  X^,  i * 0...n. 

W : 9-D  state  transition  error  vector  which  can  also 

be  interpreted  as  the  state  control  vector.  There 
are  n W's,  Wi#  i * 0...n-l. 
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A : 9-D,  Lagrange  Multiplier  vector.  There  are 

n + 1 A 1 s,  A^,  i = 0. . .n. 

y : 3-D,  Lagrange  Multiplier  vector.  There  are 

n y ' s,  y^,  i = 1. . .n. 


In  addition  to  the  "derived"  vectors,  we  will  need 
to  define  several  "input"  vectors  and  matrices.  These 
quantities  are  mathematic  expressions  of  target  dynamics  and 
measurement  error.  The  analysis  in  the  next  3 subsections 
assumes  that  the  "input"  vectors  and  matrices  are  known  for 
all  time  steps,  i = 0...n.  Section  3 will  explain  in  detail 
how  these  quantities  are  constructed  in  the  computer  program. 


TABLE  2-2  INPUT  VECTORS  AND  MATRICES 


W : 9-D  state  transition  error  mean  (or  nominal  control) 

vector.  The  expected  value  of  W.  There  are  n W's, 
W^,  i = 0. . .n-1. 

V : 3-D  measurement  error  mean  vector.  The  expected 

value  of  V.  There  are  n V's,  V^,  i = l...n. 

Z : 3-D  target  position  measurement  vector.  The  Z 

data  as  a function  of  time,  obtained  from  a 
tracking  radar  during  field  tests,  is  stored 
in  digital  format  on  a computer  tape.  There 
are  n Z's,  Z^,  i = l...n. 

Xo  : 9-D  apriori  expected  value  of  XQ. 

2 

4>  : 9 x 9-D  , nonsingular,  target  state  transition 

matrices.  4>i  takes  the  state  vector  from  time  i 

to  time  i + 1.  There  are  n $'s,  4>if  i = 0...n-l. 

Physically,  the  embody  basic  aircraft  dynamics. 


2 

Q : 9x9-D  state  transition  error  (or  state  control) 

covariance  matrix.  The  Qi  are  positive  definite; 


TABLE  2-2  INPUT  VECTORS  AND  MATRICES  (Continued) 


Qi  = (w.-Wi)(Wi-Wi)T,  i = 0...n-l.  Physically, 
measures  the  amount  that  the  expected  value  of 
the  state  transition  error  vector,  W^,  can  vary 
from  the  true  state  transition  error,  W^. 

2 

R : 3x3-D  measurement  error  covariance  matrix.  The 

Ri  are  positive  definite;  Ri  = (V.-v. ) (V.-V. )T, 

i = l...n.  Physically,  R^  measures  the  amount 
that  the  expected  v.ilue  of  the  measurement 
error,  V^,  can  vary  from  the  true  measurement 
error,  V^. 

„ „ 2 ... 

H : 3x9-D  state  to  measurement  transition  matrix. 

There  are  n H's,  , i = l...n.  Physically, 
the  matrix  takes  the  state  vector,  X^,  into 
the  coordinate  frame  of  the  measurements, 

2 

T : 9x9-D  state  transition  error  to  state  transition 

matrix,  There  are  n T's,  IV,  i = 0...n-l.  Physic- 
ally, the  I\  matrix  takes  the  state  transition 
error  vector,  W^,  into  the  coordinate  frame  of  the 
state  vector  X^. 

_ 2 

P : 9x9-0  initial  state  error  covariance  matrix, 

o _ _ 

Po  is  positive  definite;  Po  = (x_xo) (X-X0) T * 

Physically,  PQ  measures  how  much  the  expected 
value  of  the  initial  state  vector,  XQ,  can 
vary  from  the  true  initial  state,  XQ. 

The  above  definitions  are  tied  together  by  the 


dynamical  target  state  equation  (1)  and  by  the  measurement 
equation  (2) . 


xi+l  = *i  xi  + ri  wi 

i=0 . . .n-1 

(1) 

zi  - Hi  xi  + vi 

i=l...n 

(2) 

From  the  definitions  of  Q and  R we  expect  to  be 
close  to  when  Qi  is  small  and  similarly  V i to  be  close  to 
when  is  small.  In  other  words,  we  seek  estimates  of 
which  satisfy  equations  (1)  and  (2)  and  for  which  the 
corresponding  estimates  of  and  are  consistent  with  the 
size  of  and  R^ . The  following  formulation  satisfies 
these  requirements  and  has  the  added  advantage  of  being 
mathematically  solvable. 

2.2  Quadratic  Programming  Problems  (with  Linear 
Constraints) . 


l = 1/2  (x  -x  -1(x-x  ) + 

o o o o 

1/2  ”y<wi-vV1(wi-V  * i,i,rWvlwlVV1 

AAA 

find  the  estimates  X^,  W^,  of  X^,  W^,  respectively 
which  minimize  L subject  to  the  constraint  equations 

A A A 

x.^n  = $>.  x.  + r.  w. 

i+i  ii  ii 

(3) 

zi  = Hi  xi  + vi 

The  Theory  of  Optimal  Control  and  Mathematical 
Programming  (Reference  3)  proves  that  this  type  of  problem 
has  a solution  whenever  it  has  a feasible  solution,  that  is, 

AAA 

whenever  there  exists  X^,  VT,  which  satisfy  the  constraint 

A A A 

equations.  X^  = 0,  i = 0...n,  = 0,  i = 0...n,  and  = 

Z^,  i = l...n  is  a feasible  solution.  Hence  a solution  to 

formulation  (3)  exists.  Reference  3 further  proves  that 
there  necessarily  exists  vectors  A . and  y . such  that  if  we 
let  1 1 


VJ  = 0 implies  that 


which  in  turn  implies  that 


with  initial  condition 


These  equations  combine  to  yield  our  fundamental  system  of 
equations; 


Xi+1  = *iXi  - riQiri  Xi  + riWi 


Xi-1  " *i\  + “iWi  - “iV^W 


(21) 

(22) 


with  boundary  conditions 


Vo  X0 


(23) 


(24) 


Hence  the  quadratic  programming  problem,  (3),  is  reduced  to 
solving  equations  (21)  and  (22)  with  boundary  conditions 

(23)  and  (24).  Note  all  other  derived  vectors  in  (14) 

/v 

through  (20)  can  be  expressed  in  terms  of  X.,  A^,  and  input 

-1  ~ _ 

variables.  For  example,  yi  = -R^  (Z.^  - H^X^  - V^)  . 

The  difficulty  in  solving  equations  (21)  and  (22) 
is  that  the  boundary  conditions,  (23)  and  (24),  are  split  — 
one  initial  condition  and  one  end  point  or  final  condition. 
Reference  4 suggests  a technique  for  obtaining  the  final 

A 

value,  X , of  the  optimal  solution  to  equations  (21)  througxi 

n a 

(24) .  Having  determined  Xn,  we  replace  the  split  conditions 
(23)  and  (24)  with  the  two  end  point  conditions  and  can  then 

/v 

find  X^  for  all  i by  iterating  equations  (21)  and  (22)  back- 
wards in  time.  The  mathematics  are  carried  out  in  the  next 
two  subsections. 


X 

2.3a  Determination  of  the  Final  State  n - The 
Kalman  Filter.  We  first  establish  certain  properties  of  a 

homogenous  solution  X / , and  a non-homogenous  particular 

/\ 

solution,  X^,  whose  sum  is  the  optimal  solution  X^  (i.e., 

X^  1 + x^  = Xi  for  i = 0...n).  We  do  not  actually  compute 
X^*1  or  X^P  but  rather  use  certain  of  their  analytical 


! 
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properties  to  derive  recursive  relations  which  will  ulti 

/s 

mately  yield  the  end  point  vector,  X . 


Note  the  and  are  composed  of  input  matrices  and  are 
therefore  known  for  all  i. 


Consider  the  homogenous  problem 


with  boundary  conditions 


The  form  of  equations  (25)  through  (29)  suggests  the  relation 


This  is  certainly  true  for  i = 0.  The  Pi  for  i > 0 are 

proportionately  matrices  still  to  be  determined.  Substi- 
tuting relation  (31)  into  equations  (27)  and  (28),  we  arrive 

directly  at  uncoupled  equations  for  P . , X.*1,  and  v h. 


pi+l  - 

(32) 

II 

o 

cu 

po 

(33) 

^i-1h 

- I1  - Vi1  *iV 

(34) 

c- 

-x„p 

(35) 

*ih  = 

-pi*tTJih 

(36) 

matrices 

play  a fundamental  role  in  our 

analysis. 

Note  that  they  are  completely  determined  from  initial  condi- 
tion (33)  by  iterating  equation  (32)  forward  in  time. 


The  following  definitions  will  be  useful: 


pi  - + Bi-i 

(37) 

Kt  = P^iV1 

(38) 

These  definitions,  equations  (32)  and  (33) , and  some  matrix 
manipulation  yields  the  following  important  relations; 


p.  - [i  - p^l 


p.-1  = Pf1  + 


pi  = pi  - PiHiT(HipiAiT  + 


Ki  * PiHiT[Hi^iHiT  + Rirl 


P.  = (I  - K.H.)p.(I  - K.H.)T  + K.R.K.T 


(39) 

(40) 

(41) 

(42) 

(43) 


We  shall  also  find  it  useful  to  consider  the  par- 
ticular solution  to  equations  (21)  and  (22)  which  satis-  cy 
the  initial  condition 
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(44) 


In  this  case  the  sums 


A 

Xi  = 

Xih  + 

xi 

Xi  " 

X . 

1 

not  only  satisfy  equations  (21)  and  (22)  but  also  the 
boundary  conditions  (23)  and  (24).  In  other  words  the 
sums  defined  in  (46)  and  (47)  are  a representation  of  the 
solution  to  the  quadratic  programming  problem. 


The  promised  recursion  equation  leading  to  the  end 

/\ 

point  vector  X is  now  within  reach.  To  this  end  we  define 
n 

the  new  vectors; 

Yi  = XiP  + Pi$iTxiP  (48) 

Y,  = + r.  ,W.  , (49) 


yi  = ViYi-i  + ri-iwi-i 


Note  that 


Y0  " X0 


Y = 
n n 


Equation  (51)  assures  us  that  finding  an  equation  for  com- 
puting Y.  (by  forward  iteration  from  Y„)  will  lead  us  to 

the  value  of  the  end  point  vector  Xn.  Such  an  iterative 

expression  for  Y^  can  be  found  by  applying  equations  (21) 

and  (22)  to  the  X^p  and  X^p  in  equation  (48).  This  yields 
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1 


vi+l  - l*i  - pi+lAi+l»i1  xip  + 


lri  - pi+lAi+lri>  wi  + 


Ki+1  Izi+1  - vi+l>  + 


>-Bi  + pi+l  + pi+lAi+lBiJ  xi 


Using  relations  (37)  and  (39),  the  coefficient  of  the  X^ 
term  in  equation  (52)  becomes 


(52) 

P 


<Vi»iT  - pi+iAi+i»ipi»iT) 


Therefore,  after  recombining  terms  and  using  definition 
(49) , we  arrive  at  the  final  form  of  the  iterative  equation 


for  the  Y^; 


*i  = Yi  + Ki  CZi  - «iYi]  -K^. 


Yi  - *i-l  Yi-1  + Fi-1  Wi-1 


(53) 

(54) 


with  initial  condition 


Y0  = X0 


(55) 


the  are  determined  from  equations  (32),  (33),  and  (37) 


through  (43) ; 


pi  = 

(I  - KiHi)  Pi 

(56) 

Ki  - 

PiHiT  !HiPiHiT  + "i’*1 

(57) 

p.  = 

1 

ti-lPi-l*l-lT  + ri-l°iri-lT 

(58) 
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with  initial  condition 


Equations  (53)  through  (59)  taken  together  form  a closed 
system  completely  solvable  by  simple  forward  iteration. 

They  comprise  our  "forward  sweep"  through  the  data,  Z^, 

and  are  traditionally  called  the  Kalman  filter.  Clearly, 
the  Y^  have  the  important  property  that  they  are  optimal 

target  state  estimates  at  time  i=k  if  given  only  data,  Z^, 

for  i=0...k.  In  other  words  the  Y^  are  the  best  estimates 

that  can  be  obtained  on  line  (i.e. , in  real  time) . The 
variables  Y^  and  will  be  physically  interpreted  further 

in  section  2.4.  For  the  moment  we  are  concerned  with  solv- 
ing equations  (21)  and  (22)  by  backward  iteration  starting 
with  the  end  point  conditions 


X = Y (60) 

n n 

X * 0 (61) 

n 

One  approach  is  to  just  iterate  equations  (21)  and 
(22)  directly.  Another,  computationally  more  efficient 
approach  utilizes  the  Y^  and  already  computed.  This 

latter  method  comprises  the  "Backward  Sweep"  and  is  detailed 
in  the  next  subsection. 


2.3b  The  Backward  Correcting  Swvtep.  Utilizing  the 
preceeding  analysis  we  can  arrive  directly  at  the  optimal 

A 

solution  X. , X,  using  a backward  iteration  scheme  where  the 

XX  A 

X^  are  uncoupled  from  the  X^.  Using  equations  (22),  (39), 
(40),  (41),  (42),  (43),  (48), 'and  (53)  we  find 


Xi_1  = + Al  [X^  + X.p]  - HiTR1"1  [Zi  - VA] 

= (I-AiPiJ[4iTXi  + AiYi  - HjTRi~1(Z  - V±)  ] 

= [I-AiPi) I*iTXi-HiTRi‘1(Zi-Vi-HiYi) ) . (62) 
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Using  equations  (21),  (34),  (37),  (39),  and  (48)  we  find 


For  completeness  the  end  point  conditions  are  written 


Hence,  the  "Double  Sweep  Optimal  Smoother"  consists 
of  sweeping  through  the  data  in  the  forward  direction 

using  the  system  (53)  through  (59)  to  determine  and  Y^ 

i=0...n.  We  then  sweep  backwards  through  the  Z^  using 

equations  (62)  through  (65)  to  obtain  the  optimal  state 

A 

estimates  Xi>  This  backward  sweep  can  be  viewed  as  opti- 
mally correcting  the  filtered  estimates  Y^  with  the  data 

received  after  time  step  i.  The  estimates  of  the  state 
transition  error  and  measurement  error  vectors  are  found 

from  equations  (15)  and  (16) 


2.4  Complete  Double  Sweep  Equations. 

If  the  true  values  of  the  initial  state  vector,  the 
measurement  error  vector,  and  the  transition  error  vector 

equalled  Xg,  V^,  (for  all  i) , respectively,  equations 

(14)  through  (20)  or  (21)  and  (22)  would  imply  for  all  i. 


= Wi  = true  transition  error 

A 

Vi  = vi  = true  measurement  error 


X^  = true  state  vector 


(68) 

(69) 

(70) 

(71) 


L = 0 (72) 

AAA 

In  other  words,  the  optimal  estimates  X,  W,  V,  necessarily 
take  on  their  corresponding  true  values  since  in  this  case 
the  performance  criteria,  L,  takes  on  its  smallest  possible 
value,  0. 


Let  the  operator,  <5,  denote  the  variation  of  a 
vector  value  from  the  corresponding  true  value.  Then  equa- 
tions (62)  through  (72)  imply. 


6Yi  - 

«Yi  -KiH16Yi  -K± 

6Vi 

(73) 

6^  = 

+ Fi-1 

6"i-l 

(74) 

A 

6X.  = 

«Y±  -P1»1T«Xi 

(75) 

5Xi-l 

= [I-A.P.] [$.6X. 

+ Ai6Yi  + HiTRi"16Vi] 

(76) 

A 

6W.  = 

- Q1rYSx1 

(77) 

li 

•H 
< > 
«o 

Hi«*i 

(78) 
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We  assume  ttuVc  the  variations  of  V^,  and  Xq  from  their 

corresponding  true  values  are  given  as  in  the  definitions  of 
Section  2.1  and  are  entirely  random; 


(6. . is  the  Krenecker  delta) 


From  equations  (73)  through  (84)  we  get  the  important  results, 


Using  equations  (85)  through  (90)  it  is  straight  forward, 
although  sometimes  lengthy,  to  show; 


6X6X 


Using  (91),  (92),  and  (95)  we  get  the  recursive  relations 


6Yi6YiT  = (I"KiHi)  6Yi6YiT  d-KiHi)7 


(100) 


(101) 


(102) 


Comparing  equations  (100)  through  (102)  with  equations  (33) 
(37),  and  (43)  we  arrive  at 


(103) 


(104) 


Hence,  the  P (P)  are  the  covariance  of  the  variations  in  Y 

(Y)  due  to  the  variations  6V,  6W.  Continuing  along  these 
lines,  define; 


(105) 


(106) 


(107) 


(108) 


I 


SRi  = 6Vi5ViT 


SP,  SQ,  SR  are  the  covariances  of  the  errors  in  the  estimates 

AAA 

X,  W,  V,  respectively.  Using  equations  (73)  through  (78)  and 
(91)  through  (99)  we  easily  find; 


' 


i 


spi  = 

P.  - p.$.ta.$.p. 

l li  ill 

(109) 

SQi  - 

q.  - Q.r.A.r.TQ. 

1 1 1 1 1 W1 

(110) 

SR.  = 

HiSPiHiT 

(HIT 

Ai-1  = 

(I  - p-a.)11  A.$.  (I  - P^.) 

+ A±  (I  - P^.) 

(112) 

A 

n 

0 

(113) 

The  forward  sweep  equations,  (53)  through  (59),  and  the 
complete  backward  sweep  equations,  (62)  through  (67)  and 
(109)  through  (113),  form  the  "Double  Sweep  Optimal  Smoother." 
Note  that  the  double  sweep  equations  require  values  for  the 

input  variables  XQ,  PQ,  W,  V,  R and  Q.  The  next  section 

describes  how  these  quantities  are  computed. 


' 
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3.  COMPUTATION  OF  INPUT  VARIABLES 


In  order  for  the  iterative  process  of  the  double 
sweep  smoother  to  be  initialized  and  maintained,  the  XQ, 

PQ»  wj/  V^,  R^,  Q^,  and  must  be  supplied  for  all  i.  A 
complete  analysis  of  these  quantities  is  given  in  "Adaptive 
Estimation"  (Reference  5).  A summary  is  given  below. 


3.1 


Initialization  Inputs 


P0  and 


All  nine  components  of  Xq  are  set  equal  to  zero. 

PQ  has  all  off  diagonal  elements  set  to  zero.  Its  first 
three  diagonal  elements,  corresponding  to  initial  position 
uncertainty  are  set  very  large.  The  initial  velocity  uncer- 
tainty is  large  but  recognizes  that  the  aircraft  is  subsonic 
while  the  initial  acceleration  uncertainty  assumes  that  the 
aircraft  is  not  in  a severe  maneuver  at  initial  detection. 
Smoother  (and  filter)  estimates  are  not  very  sensitive  to 
small  chnages  in  Pq  as  long  as  the  diagonal  elements  are 
large. 


0 

0 


(3000)  I 


3x3 


0,  - 0,  , 
3x3  3x3 


,240>  *3x3  °3x3 


°3x3  (25>2l3x3 


The  assumed  values  of  the  initial  state  and  the  correspon- 
ding blocks  of  Pq  given  above  can  easily  be  changed  by  the 
user. 
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Since  most  of  the  tracking  data  that  we  use  is 
supplied  in  a fixed  inertial  cartesian  coordinate  frame,  the 
smoother  assumes  that  the  Z ^ are  expressed  in  the  same  frame 

as  the  state  vector,  although  it  can  be  ear ' ly  modified  to 
account  for  any  measurement  frame.  The  model  of  the  meas- 
urement errors  in  the  program  accounts  for  constant  variance  angular 
errors  and  also  constant  variance  linear,  glint  type,  errors  in  both 
azimuth  and  elevation  and  for  a fixed  variance  linear  error  in  range. 
The  covariance  matrix  of  these  errors  is  computed  in  the 
radar  line  of  sight  frame  and  then  expressed,  via  a similar- 
ity transformation,  in  the  fixed  reference  frame.  The 
reference  to  sensor  frame  coordinate  transformation  TRS^  is 

computed  at  each  i using  the  latest  estimate,  Y^,  of  the 

target  position  vector  during  the  forward  sweep.  The  meas- 
urement related  matrices  are; 


TRSi  " -2Y4 


Hi  = 


LYf |y| 


100  000  000 
010  000  000 
001  000  000 


” it  j Yg | 
-1Y3Yt | y| | Yg | 


•Yt|y  3Yt|y| 


1Yt |Yg|  0 

-2Y3Yt I Y I | Yg | |Yg| t)y| 


(the  subscript  i's  on  the  Y's  are  omitted  for  notational 
convience) . Where 


LY2  + 2Y2  + 3Y2 


I I / 12  22 

and  upper  left  superscript  denotes  component. 
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where 


sigrxs^  = 

2 

sigmr 

sigrysi  = 

(stdazl) 

sigrzs^  = 

(stdell) 

( |.Y^  | stdaz2)  2 
( |y^ |stdel2) 2. 


sigmr,  stdazl,  stdaz2,  stdell,  stdel2,  are  constants  set  by 
the  user.  Nominal  values  are  5 (meters),  1 (meter) , .0005 
(mils  t 1000),  1 (meter),  .0005 (mils  t 1000),  respectively. 

We  assume  that  the  tracking  radar  which  generated 
the  was  well  calibrated  and  therefore  had  zero  bias.  In 

other  words, 

V.  = 0 i=l...n. 


3.3  The  Target  Dynamical  Inputs  $,  Q,  T,  and  W. 


4k  represents  the  dynamics  of  an  aircraft  which  is 

controlled  at  time  step  i through  its  acceleration  rate, 
riWi*  Alternatively,  we  can  look  on  4k  as  representing  an 

aircraft  whose  acceleration  is  constant  over  any  particular 
transition,  i+i-t-1 , where  the  error  in  this  representation  is 
given  by  r\W^. 


4> 


i 


3x3 

I3x3^t 

,5I3x3 

3x3 

I3x3 

I3x3^t 

3x3 

°3x3 

I3x3 
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where  At  = time  step  between  iterations.  Note  that  <t>  is 
constant  over  all  i. 

The  computations  of  the  inputs  and  Qi  are  com- 
plicated in  that  they  are  physically  related  to  the  aircraft 
body  frame  variables  of  thrust  rate,  g rate,  and  roll  rate 
and  their  corresponding  covariances.  The  transition  from 
body  frame  dynamics  to  reference  frame  dynamics  is  worked 
out  in  Reference  5.  At  any  particular  instant,  a circle  is 
matched  to  the  target's  flight  path  with  the  properties  that 
the  component  of  target  acceleration  perpendicular  to  the 
velocity  vector  points  towards  the  circle's  center.  The 
target's  body  frame  has  its  first  component  axis  along  the 
velocity  vector  and  its  second  component  axis  along  the 
normal  acceleration  vector.  It  is  also  called  the  Frenet 
frame.  r is  the  body,  or  Frenet  frame,  to  reference  frame 
coordinate  transformation.  It  is  computed  at  each  i based 
on  the  latest  available  state  estimate . r ' s matrix  compon- 
ents are; 


11 

r 

= 

4 

Y ^ V 

21r 

= 

5 

°Y  -r  V 

31r 

= 

6Y  t V 

12r 

= 

(7Y  - 4Y  at) 

t gn 

22r 

= 

(8Y  - 5Y  at) 

t gn 

32p 

= 

(9Y  - 6Y  at) 

t gn 

13r 

= 

21r  32r  _ 31r 

22r 

23r 

= 

12r  31f  _ llr 

32r 

33r 

_ 

lip  22p  _ 12p 

21r 

where 

V = (4Y2  + 5Y2  + 6y2)1/2 
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where 


QAP 


0 

(C2At)2 

0 


: ,) 

(gn*C3At) 2 ' 


C^,  C2f  C3  are  constants  set  by  the  user  corresponding  to 

the  one  standard  deviation  (STD)  of  thrust 

rate,  g rate,  and  roll  rate,  respectively.  Nominal  values 

3 3 

are  1. 0 (meters/sec  ),  7. 0 (meters/sec  ),  and  1 . 0 (radians/sec 
respectively.  Note,  for  notational  convenience  the  i 
subscripts  have  been  omitted  in  all  the  above  equations. 

3.4  Remarks  on  Nonlinearities. 

The  input  variables  can  in  principle  be  defined  in 
a variety  of  ways.  For  example,  they  could  each  be  referred 
to  any  number  of  possibly  different  coordinate  systems. 
Unfortunately,  the  nature  of  the  tracking  problem  will 
result  in  any,  but  the  most  simple  minded,  formulation 
having  nonlinear  portions.  The  investigator  can  influence 
where  these  nonlinearities  occur.  In  our  formulation  they 
have  been  shifted  to  the  coordinate  transition  matrices  TRS 
and  T. 


Nonlinearities  have  two  effects.  On  the  forward 
sweep,  T and  TRS  depend  on  the  most  recent  state  estimates, 
Y^,  for  their  computation.  Therefore  the  cannot  be  a 

priori  computed  but  must  be  determined  simultaneously  with 
the  . Secondly,  we  must  change  our  interpretation  of  the 

6 operator  in  equations  (73)  through  (108)  from  that  of 
variation  to  that  of  small  variation. 

The  nonlinearities  raise  some  question  as  to  the 

adequacy  of  the  W and  Q computations,  particularly  for 
severely  jinking  targets  where  lags  in  the  forward  sweep 
estimates  may  become  large.  Conceivably  we  can  reduce  the 
lags  by  increasing  Q,  that  is  increasing  C^,  C2,  C3,  and/or 

running  the  smoother  back  and  forth  over  the  data  several 

times,  each  time  starting  with  Xq  and  PQ  from  the  previous 
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backward  sweep.  Doubtless  the  reader  can  think  of  other 
schemes  for  reducing  the  effects  of  nonlinearities. 


4 . USERS ' GUIDE 

The  program  uses  Top  Down  Structured  Programming 
(TDSP)  and  is  written  in  simple,  machine  independent  FOR- 
TRAN. In  line  with  TDSP,  the  coding  uses  only  five  logical 
structures,  is  self-documented  (requiring  no  flow  charts) 
and  is  completely  straightforward  (avoiding  "programming 
tricks") (Reference  6).  A brief  description  of  the  program's 
overall  structure  and  a few  remarks  on  input  data  are  all  a 
perspective  user  will  need  for  a guide. 

4.1  Overall  Structure. 

A schematic  diagram  of  the  functional  hierarchy  is 
given  in  Figure  4.1. 
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The  first  level  consists  of  three  independent 
programs,  the  forward  sweep,  sort,  and  backward  sweep 
programs.  The  forward  sweep  reads  the  tracking  data  from  a 
user  selected  storage  device  (e.g.,  FASTRAND,  tape,  disc), 
filters  it,  and  writes  the  results  on  user  selected  devices. 
The  sort  program  reverses  the  chronological  order  of  the 
filtered  output  in  preparation  for  the  backward  sweep.  The 
user  is  urged  to  consult  his  own  computer  facility  for  a 
machine  optimized  sort  routine.  For  example,  AMSAA’s  Air 
Defense  Evaluation  Branch  uses  a sort  program  optimized  for 
the  Univac  1108  consisting  of  one  executive  command,  0MISD* 
LlB$ . SORTSDF  12.,  12 . , #records , #chars/per,  KEY/127/4.D, 
KEY/131/2. A.  The  backward  sweep  reads  the  filtered  output 
in  reverse  chronological  order  and  writes  the  final  smoothed 
results  on  user  selected  devices.  The  user  may  wish  to  use 
SORT  again  since  the  smoothed  results  are  in  reverse  chrono- 
logical order. 

The  next  functional  level  is  the  FILTER  subroutine. 
It  directs  the  iteration  of  equations  (53)  through  (59) , and 
contains  the  data  statements  which  are  fundamental  to  the 

computation  of  the  input  variables  XQ,  PQ,  R,  and  Q.  The 

next  two  levels  perform  computations  specific  to  one  itera- 
tion of  equations  (53)  through  (59)  for  Y^  and  and  of 

equations  (62)  through  (67)  and  (109)  through  (113)  for  A^ 

and  corresponding  to  the  forward  and  backward  sweep 

programs,  respectively.  Since  considerably  less  computation 
is  required  on  the  backward  sweep  (compared  to  the  computa- 
tion required  on  the  forward  sweep) , it  is  nearly  all 
accomplished  in  the  main  program  element.  In  fact,  there  is 
only  one  subroutine  in  the  backward  sweep  program.  On  the 
lowest  functional  level,  we  find  the  utility,  matrix  inver- 
sion, subroutine  CHLSKI  (choleski  inversion)  which  in  turn 
calls  the  subroutines  DECOM  and  SUBS. 

4.2  User  Inputs. 

The  file  containing  the  radar  measurements 

(RAWDAT,  device  11)  must  have  a header  line  giving  the  path 
ID  (RUNID)  and  the  number  of  time  steps  (MAXDAT)  contained 
on  the  file.  The  necessary  formats  are  found  in  the  forward 
sweep  main  program.  The  filter  output  is  stored  in  a user 
assigned  file  (FLTOUT,  device  12) . The  filter  out  is  coded 
with  two  sorting  indices;  NCOUNT  and  SORT.  SORT  is  a one 
dimensional  array.  The  user  selected  sorting  routine  should 
sort  in  decreasing  numerical  order  on  NCOUNT  and  in  normal 
ascending  order  on  SORT.  The  backward  sweep  program  reads 
the  sorted  (reverse  chronological  order)  data  from  FLTOUT 
and  writes  the  smoothed  data  in  a user  assigned  file  (SMTOUT, 
device  13) . 
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The  data  time  step,  DELTAT,  must  be  set  in  each  of 
the  two  main  program  elements.  Subroutine  INTFLT  contains 
all  the  remaining  variables  that  are  to  be  set  by  the  user. 
They  are; 

CONE,  the  STD  of  target  thrust  rate, 

CTWO,  the  STD  of  target  g rate, 

CTHREE,  the  STD  of  target  roll  rate, 

SIGMR,  the  STD  of  range  measurement  error, 

STDAZl,  the  STD  of  linear  azimuth  measurement  error, 

STDAZ2,  the  STD  of  angular  azimuth  measurement  error, 

STDELl , the  STD  of  linear  elevation  measurement  error, 

STDEL2,  the  STD  of  angular  elevation  measurement  error, 

PVAL(i),  i = 1...9,  are  the  STDs  of  the  initial  state 
vector  in  the  X,  Y,  Z directions  of  position,  velocity,  and 
acceleration,  respectively. 

4.3  Identification  of  Program  Names. 

This  section  identifies  the  FORTRAN  names  of  the 
principal  variables  used  in  the  program  with  the  correspond- 
ing symbols  used  in  the  theory  of  sections  2 and  3. 


Program  Name 


BARN 


Theory  Name 


CAP LAM 


DELTAT 


HKAL 


LAMDA 


PKAL 


QKAL 


P,  P 
r qap  r1 


r q r 


Theory  Name 


Program  Name 


RESDUL 


SMTBRN 


SMTP 


SMTQ 


ZMEAS 


\ 
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Table 

Title 

Page 

A-l 

Forward  Sweep  Main  Program 

41 

A-2 

Backward  Sweep  Main  Program 

44 

A- 3 

Subroutine 

CHLSKI 

49 

A- 4 

Subroutine 

CORECT 

50 

A- 5 

Subroutine 

CTRF 

52 

A- 6 

Subroutine 

CTRS 

55 

A- 7 

Subroutine 

DECOM 

57 

A-8 

Subroutine 

EXTRAP 

58 

A- 9 

Subroutine 

FILTER 

60 

A-10 

Subroutine 

INTFLT 

61 

A- 11 

Subroutine 

LAMDAS 

64 

A-12 

Subroutine 

NBAR 

67 

A-13 

Subroutine 

Q 

69 

A- 14 

Subroutine 

R 

71 

A-15 

Subroutine 

SUBS 

73 
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US  Army  Concepts 
Analysis  Agency 
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and  Evaluation  Agency 
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